import roomba
import msvcrt
import time
import math
import sys

if len(sys.argv) > 1:
    PORT = int(sys.argv[-1])
else:
    PORT = 10

print "Using port", PORT

def RoombaRemote():

    thebot = roomba.RoombaRobot(PORT)
    x = 0.0
    y = 0.0
    ang = 0.0
    xmark = 0.0
    ymark = 0.0
    delay = 0
    xmin = 10000000
    ymin = 10000000
    dmin = 10000000
    tolerance = 70
    
    #Roomba States
    SEEK_GOAL = 0
    ENCIRCLE = 1
    SEEK_NEAREST_POINT = 2
    DONE = 3
    robotState = SEEK_GOAL
    
    goalx = 2000
    goaly = 0
    time.sleep(.2)
    while True:
        bump = 0
        #time.sleep(.1)
        #(x,y,thr,lftBmp,rgtBmp) = thebot.GetPoseReal()
        thebot.roomba.ser.write( chr (149))
        thebot.roomba.ser.write( chr (3))
        #distance, then angle, then bump stuff
        thebot.roomba.ser.write( chr (19))
        thebot.roomba.ser.write( chr (20))
        thebot.roomba.ser.write( chr (7))
        #time.sleep(.2)
        #a = thebot.roomba.ser.read(size=1)
        #print ord(a)
        r = thebot.roomba.ser.read(size=5)
        r = [ ord(c) for c in r]
        
        #if(lftBmp or rgtBmp):
        #    thebot.roomba.drive(300,0)
        
        if msvcrt.kbhit():
            c = msvcrt.getch()
            if c == 'i':
                print 'i'
                thebot.roomba.drive(300,2001)
            if c == 'k':
                print 'k'
                thebot.roomba.drive(-150, 2001)
            if c == 'j':
                print 'j'
                thebot.roomba.drive(300, 300)
            if c == 'l':
                print 'l'
                thebot.roomba.drive(300, -300)
            if c == 'u':
                print 'u'
                thebot.roomba.drive(300,0)
            if c == 'w' or c == ' ':
                print 'w'
                thebot.roomba.drive(0,0)
            if c == 's':
                print "Enabling Sensor Stream"
                # Request a stream
                thebot.roomba.ser.write( chr(148) )
                # with N sensor packets (N=1)
                thebot.roomba.ser.write( chr(1) )
                # We want packt 17 (IR Byte)
                thebot.roomba.ser.write( chr(17) )
                #thebot.roomba.ser.write( chr(8) )
            if c == 'a':
                print "Pausing Sensor Stream"
                # Pause stream
                thebot.roomba.ser.write( chr(150))
                thebot.roomba.ser.write( chr(0))
            if c == 'z':
                print "Resuming Sensor Stream"
                # Resume stream
                thebot.roomba.ser.write( chr(150))
                thebot.roomba.ser.write( chr(1))
            if c == 'x':
                print "Sending IR transmit request"
                thebot.roomba.ser.write( chr(151))
                thebot.roomba.ser.write( chr(85))
                #time.sleep(0.2)
                #print "Reading IR byte"
                thebot.roomba.ser.write( chr(142))
                #thebot.roomba.ser.write( chr(1))
                thebot.roomba.ser.write( chr(17))
            if c == 'v':
                print "Starting IR transmit script sequence"
                # Start script
                thebot.roomba.ser.write( chr(152))
                # length of script
                thebot.roomba.ser.write( chr(3))
                # IR Send
                thebot.roomba.ser.write( chr(151))
                # IR data byte
                thebot.roomba.ser.write( chr(85))
                # End script
                thebot.roomba.ser.write( chr(153))
                # Start script
                thebot.roomba.ser.write( chr(153))
            if c == 'm':
                #Start mouse demo
                thebot.roomba.ser.write( chr(136))
                thebot.roomba.ser.write( chr(3))
            if c == 'n':
                #Stop current demo
                thebot.roomba.ser.write( chr(136))
                thebot.roomba.ser.write( chr(255))
                #Put roomba back in full mode
                thebot.roomba.ser.write( chr(132))
            if c == 'r':
                x = 0
                y = 0
                ang = 0
            if c == 'b':
                startBugStream(thebot)
            if c == 'q':

                thebot.roomba.ser.close()
                break
        #start = False
        #ctr = 0
        # Print the sensor outputs
        #print r
        #for i in range(0, len(r)-1):
            #print 'r[i]: ', r[i]
            #if (r[i] == 19 and r[i+1] == 8 and len(r) > (i + 9)):
                #distance = roomba.twosComplementInt2bytes(r[i+3], r[i+4])  
                #angle = roomba.twosComplementInt2bytes(r[i+6],r[i+7])
                #leftbump = roomba.bitOfByte( 1, r[i+9])
                #rightbump = roomba.bitOfByte(0, r[i+9])
        distance = roomba.twosComplementInt2bytes(r[0], r[1])  
        angle = roomba.twosComplementInt2bytes(r[2],r[3])
        leftbump = roomba.bitOfByte( 1, r[4])
        rightbump = roomba.bitOfByte(0, r[4])
        #if(abs(ang) > 360):
        #    thebot.roomba.drive(0,0)
        if(robotState == SEEK_GOAL and (leftbump == 1 or rightbump == 1)):
            bump = 1
            xmark = x
            ymark = y
            delay = 10
        #print 'r[i+3] ', r[i+3], 'r[i+4]', r[i+4], 'distance ', distance    
        ang += angle
        if (ang > 360):
            ang -= 360
        if (ang < 0):
            ang += 360
        x += math.cos(math.radians(ang))*distance
        y += math.sin(math.radians(ang))*distance

        #print 'x: ', x, 'y: ', y, 'angle: ', ang
        #print 'xmark: ', xmark, 'ymark: ', ymark
        #print 'xmin: ', xmin, 'ymin: ', ymin
        if (bump == 1 and robotState == SEEK_GOAL):
            print 'THE ROOMBA BUMPED'
            robotState = ENCIRCLE
            mouseDemo(thebot)
            delay = 50

        #keep track of minmum point
        if (robotState == ENCIRCLE):
            if ( math.sqrt((goalx-x)**2 + (goaly-y)**2) < dmin):  
                dmin =  math.sqrt((goalx-x)**2 + (goaly-y)**2)
                xmin = x
                ymin = y		

        if (robotState == ENCIRCLE and abs(xmark -x) < tolerance and abs(ymark -
            y) < tolerance and delay < 0):
            #endDemo(thebot)
            #thebot.roomba.drive(0,0)
            robotState = SEEK_NEAREST_POINT
            print 'THE ROOMBA RETURNED'
            
            # The mouse script seems to know when it's gone all the way around
            # an object, so to prevent this, we turn back toward the object and
            # restart the demo
            endDemo(thebot)
            turnScript(thebot, -30)            
            mouseDemo(thebot)
            #u = 87
        if(robotState == SEEK_NEAREST_POINT and abs(xmin-x) < tolerance and
                abs(ymin - y) < tolerance ):
            print 'THE ROOMBA GOT TO MIN POINT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1'
            robotState = SEEK_GOAL

            endDemo(thebot)
            thebot.roomba.drive(0,0)
            dy = y - goaly
            dx = x - goalx
            turnAngle = math.degrees(math.atan2(dy,dx)) + 180
            turnAngle = turnAngle - ang
            turnScript(thebot, turnAngle )

            #now we are pointing at the goal, go there
            thebot.roomba.drive(300, 2001)
                    

        if (robotState == ENCIRCLE):
            delay -= 1

        # STOP if we are at the goal
        if (robotState == SEEK_GOAL and abs(goalx - x) < tolerance and
                abs(goaly - y) < tolerance):
            thebot.roomba.drive(0,0)
            robotState = DONE
            print 'THE ROOMBA HAS ARRIVED.'


def startBugStream(thebot):
    #start stream for distance and angle
    thebot.roomba.ser.write( chr(148))
    #Number of Bytes
    thebot.roomba.ser.write( chr(3))
    #Distance
    thebot.roomba.ser.write( chr(19))
    #Angle
    thebot.roomba.ser.write( chr(20))
    #Bump Sensor
    thebot.roomba.ser.write( chr(7))

def mouseDemo(thebot):
    #Start mouse demo
    thebot.roomba.ser.write( chr(136))
    thebot.roomba.ser.write( chr(3))

def endDemo(thebot):
    #Stop current demo
    thebot.roomba.ser.write( chr(136))
    thebot.roomba.ser.write( chr(255))
    #Put roomba back in full mode
    thebot.roomba.ser.write( chr(132))

def turnScript(thebot, turnAngle):
    #Start the script
    print "Start the script"
    thebot.roomba.ser.write( chr(152))
    #Length of script
    print "Length of script"
    thebot.roomba.ser.write( chr(13))
    #Drive
    print "Drive"
    thebot.roomba.ser.write( chr(137))
    #At 300 mm/s
    print "At 300 mm/s"
    if (turnAngle < 0):
        high, low = roomba.toTwosComplement2Bytes(-300)
    else:
        high, low = roomba.toTwosComplement2Bytes(300)
    thebot.roomba.ser.write( chr(high))
    thebot.roomba.ser.write( chr(low))
    #Turn Clockwise
    print "Turn Clockwise"
    thebot.roomba.ser.write( chr(0))
    thebot.roomba.ser.write( chr(0))
    #WAIT FOR ANGLE
    print "WAIT FOR ANGLE"
    thebot.roomba.ser.write( chr(157))
    high1, low1 = roomba.toTwosComplement2Bytes(int(turnAngle))
    thebot.roomba.ser.write( chr(high1))
    thebot.roomba.ser.write( chr(low1))

    #STOP THE ROOMBA
    print "STOP THE ROOMBA"
    thebot.roomba.ser.write( chr(137))
    thebot.roomba.ser.write( chr(0))
    thebot.roomba.ser.write( chr(0))
    thebot.roomba.ser.write( chr(0))
    thebot.roomba.ser.write( chr(0))
    #print "READ A SENSOR VALUE"
    #thebot.roomba.ser.write( chr(142))
    #thebot.roomba.ser.write( chr(7))

    #Play Script
    print "Play Script"
    thebot.roomba.ser.write( chr(153))
    # Wait for the turn to finish
    # TODO: wait in a better way
    print "Waiting for script to finish"
    time.sleep(3) 
    # This needs to wait until the script finished...
    #a = thebot.roomba.ser.read(size=1)
    print "Script Assumed Finished"

''' 
def findGoal(x,y):

    thebot = roomba.RoombaRobot(PORT)

    thebot.roomba.resetPose()
    thebot.roomba.reset
    goal = (x,y)

    while True:
        [x,y,thd], bump = self.getData()
        thr = math.radians(thd)


        dx = goal[0]-x
        dy = goal[1]-y
        distanceToGoal = math.sqrt(dx*dx+dy*dy)
        #mytheta = thr - math.pi
        try:
            absoluteAngle = math.atan2(dy,dx)
            angleToGoal =  absoluteAngle - thr 
        except:
            angleToGoal = 0
        if angleToGoal < -1*math.pi:
            angleToGoal += 2*math.pi
        if angleToGoal > math.pi:
            angleToGoal -= 2*math.pi

        # This stops if a bump is encountered - a pretty basic plan
        if bump[0] == True or bump[1] == True:
            print 'BUMP!',
            print '  [Left bump sensor:',  bump[0], ']  ',
            print '  [Right bump sensor:', bump[1], ']  '
            robotTask == BUMP


        if robotTask == TRACKGOAL:
            if angleToGoal > 0:
                self.roomba.drive(200, 300)
            else:
                self.roomba.drive(200, -300)

        if robotTask == BUMP                
            if bump[0]:
                self.roomba.drive(-200,0)
            elif bump[1]:
                self.roomba.drive(200,0)
            else:
                robotTask == TRACKGOAL

'''
def reset():
    thebot = roomba.RoombaRobot(PORT)
    time.sleep(.2)
    thebot.roomba.ser.write( chr(128) )
    thebot.roomba.ser.write( chr(131) )
    thebot.roomba.ser.write( chr(128) )
    thebot.roomba.ser.write( chr(131) )

def rotTest():
    thebot = roomba.RoombaRobot(PORT)
    time.sleep(.2)
    thebot.SetPose(0,0,0)
    thebot.roomba.drive(75,0)
    time.sleep(4*2.79)
    thebot.roomba.drive(0,0)

def square():
    thebot = roomba.RoombaRobot(PORT)
    #thebot.roomba.start()
    time.sleep(.2)
    #thebot.roomba.ser.write( chr(128) )
    #thebot.roomba.ser.write( chr(132) )
    currAng = 0
    state = 0
    forwardVel = 150
    turnVel = 75
    thebot.SetPose(0,0,0)
    thebot.roomba.drive(forwardVel,2001)
    while True:
        (x,y,thr,lb,rb) = thebot.GetPoseReal()
        if(x > 66.68 and state == 0):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > math.pi/2.0 and state ==1):
            state += 1
            thebot.roomba.drive(forwardVel, 2001)
        if(y > 66.68 and state == 2):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > math.pi and state == 3):
            state += 1
            thebot.roomba.drive(forwardVel,2001)
        if(x < 0 and state == 4):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > 3.0/2.0*math.pi and state == 5):
            state += 1
            thebot.roomba.drive(forwardVel,2001)
        if(y < 0 and state == 6):
            thebot.roomba.drive(turnVel,0)
            state += 1
        if(thr > 2*math.pi and state == 7):
            thebot.roomba.drive(0,0)
            state = 0
            
            
         
    
    
        #(x,y,thr,lb,rb) = thebot.GetPoseReal()
        print x,y,thr,lb,rb
        #if(x > 66.68):
        #    thebot.roomba.drive(0,0)
        if msvcrt.kbhit():
            c = msvcrt.getch()
            if c == 'q':
                break
            if c == 'w':
                thebot.roomba.drive(0,0)
            if c == 'y':
                thebot.roomba(-150,0)
            if c == 'm':
                thebot.roomba.drive(150,2001)
                state = 0
                thebot.SetPose(0,0,0)

if __name__ == '__main__':      # are we at the command line
    reset()
    #square()
    RoombaRemote()
